import java.util.ArrayList;
import java.util.List;

import javax.media.opengl.GL;
import javax.media.opengl.GL2;
import javax.media.opengl.GLAutoDrawable;

public class Triangle extends PhysicsObject {
	
	/** Determines which triangle/trainagle collision method to use. */
	private static final boolean USE_DEVILLIERS = false;

	//Note there MUST be as many colors as vertices
	protected Vector2f[] vertices = {
			new Vector2f(0, 0),
			new Vector2f(1, 0),
			new Vector2f(0, 1)
	};
	protected float[][] colors = {
			new float[] {1, 1, 0},
			new float[] {0, 1, 1},
			new float[] {1, 0, 1}
	};
	
	private Vector2f[] vertexCache;
	private Vector2f[] normalCache;
	
	private class Renderable extends SceneGraphNode {
		public Renderable(boolean pickable) {
			super(pickable);
			physics = Triangle.this;
		}
		
		public void renderGeometry(GLAutoDrawable drawable) {
			GL2 gl = drawable.getGL().getGL2();
			gl.glBegin(GL.GL_TRIANGLES);
			for (int i = 0; i < vertices.length; i++) {
				gl.glColor3fv(colors[i], 0);
				gl.glVertex2f(vertices[i].x, vertices[i].y);
			}
			gl.glEnd();
		}
	}

	public Triangle(float size) {
		this(size, true);
	}

	public Triangle(float size, boolean pickable) {
		centerOfMass.x = centerOfMass.y = size / 3;
		inverseMomentOfInertia = 1 / (float)(Math.pow(size, 4) / 18);
		renderable = new Renderable(pickable);
		renderable.scale = size;
		renderable.CoMX = centerOfMass.x;
		renderable.CoMY = centerOfMass.y;
		inverseMass = 2 / size;
	}
	
	public void clearCaches() {
		vertexCache = null;
		normalCache = null;
	}
	
	/** Returns the vertices of the triangle, respecting orientation and position. **/
	public Vector2f[] getVertices() {
		if (vertexCache == null) {
			vertexCache = new Vector2f[vertices.length];
			Vector2f tmp = new Vector2f();
			for (int i = 0; i < vertices.length; i++) {
				tmp.x = vertices[i].x * renderable.scale;
				tmp.y = vertices[i].y * renderable.scale;
				tmp.sumScale(centerOfMass, -1);
				vertexCache[i] = new Vector2f();
				vertexCache[i].x = (float)(Math.cos(orientation) * tmp.x - Math.sin(orientation) * tmp.y);
				vertexCache[i].y = (float)(Math.sin(orientation) * tmp.x + Math.cos(orientation) * tmp.y);
				vertexCache[i].sum(centerOfMass);
				vertexCache[i].sum(position);
			}
		}
		return vertexCache;
	}
	
	public Vector2f[] getNormals() {
		if (normalCache == null) {
			Vector2f[] vertices = getVertices();
			normalCache = new Vector2f[vertices.length];

			for (int i = 0; i < vertices.length; i++) {
				normalCache[i] = new Vector2f(vertices[(i+1)%vertices.length]);
				normalCache[i].sumScale(vertices[i], -1);
				normalCache[i].normalize();
				float tmp = normalCache[i].x;
				normalCache[i].x = normalCache[i].y;
				normalCache[i].y = -tmp;
			}
		}
		return normalCache;
	}
	
	public static CollisionInfo getCollision(HalfSpace a, Triangle b) {
		Vector2f[] vertices = b.getVertices();
		float[] distances = new float[vertices.length];

		for (int i = 0; i < vertices.length; i++)
			distances[i] = a.normal.dot(vertices[i]) - a.intercept;

		int minIndex = 0;
		for (int i = 1; i < distances.length; i++)
			if (distances[i] < distances[minIndex])
				minIndex = i;
		if (distances[minIndex] >= 0)
			return null;

		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distances[minIndex];
		cInfo.normal = new Vector2f(a.normal);
		cInfo.positionA = new Vector2f(vertices[minIndex]);
		cInfo.positionA.sumScale(cInfo.normal, cInfo.depth);
		cInfo.positionB = new Vector2f(vertices[minIndex]);
		return cInfo;
	}

	public static CollisionInfo getCollision(Circle a, Triangle b) {
		Vector2f[] vertices = b.getVertices();
		Vector2f[] normals = b.getNormals();
		float[] distances = new float[vertices.length];

		for (int i = 0; i < vertices.length; i++) {
			Vector2f tmp = new Vector2f(a.position);
			tmp.sumScale(vertices[i], -1);
			distances[i] = tmp.dot(normals[i]) - a.radius;
		}
		// beginning of modified code//
		int maxIndex = 0;

		for (int i = 1; i < distances.length; i++) {
			if (distances[maxIndex] < distances[i]) {
				maxIndex = i;
			}
		}

		// if this happens, there is no collision, exit early
		if (distances[maxIndex] >= 0) {
			return null;
		}

		boolean collision = false;
		for (int i = 0; i < distances.length; i++) {
			// distance formula to see if vertex is inside the circle
			// distanceFormula = math.sqrt((x2-x1)^2 + (y2-y1)^2)
			float distanceVertexToCircleCenter = (float) Math.sqrt(Math.pow(
					a.position.x - b.getVertices()[i].x, 2)
					+ Math.pow(a.position.y - b.getVertices()[i].y, 2));

			// there was a "corner" collision if this is true
			if (distanceVertexToCircleCenter < a.radius) {
				collision = true;
			}
		}

		// no corner collision, check for side collision
		if (collision == false) {
			// count the number of iterations
			int count = 0;
			for (int i = 0; i < distances.length; i++) {
				// odd boundary cases where one distance is less than radius,
				// but two
				// half spaces are intersecting the circle
				if (-distances[i] < a.radius
						&& -distances[(i + 1) % 3] > 2 * a.radius
						&& -distances[(i + 2) % 3] > a.radius
						&& -distances[(i + 2) % 3] < 2 * a.radius) {
					return null;
				} else if (-distances[i] < a.radius
						&& -distances[(i + 1) % 3] > a.radius
						&& -distances[(i + 1) % 3] < 2 * a.radius
						&& -distances[(i + 2) % 3] > 2 * a.radius) {
					return null;
				}
				// regular side collisions
				else if (-distances[i] < a.radius) {
					count++;
				}
			}

			// if there was only one iteration, there must be a side collision
			if (count == 1) {
				collision = true;
			}
		}

		// resolve outstanding collision
		if (collision) {
			CollisionInfo cInfo = new CollisionInfo();
			cInfo.depth = -distances[maxIndex];
			cInfo.normal = new Vector2f(normals[maxIndex]);
			cInfo.normal.scale(-1);
			cInfo.positionA = new Vector2f(a.position);
			cInfo.positionA.sumScale(cInfo.normal, a.radius);
			cInfo.positionB = new Vector2f(a.position);
			cInfo.positionB.sumScale(cInfo.normal, a.radius - cInfo.depth);
			return cInfo;
		}

		// there were no collisions, nothing to resolve or return
		return null;
	}
	
	public static CollisionInfo getCollision(Triangle a, Triangle b) {
		if (USE_DEVILLIERS) {
			return getCollisionDevilliers(a, b);
		} else {
			CollisionInfo info = checkForVertexInside(a, b);
			if (info == null) {
				info = checkForVertexInside(b, a);
				if (info != null) {
					info.flip();
				} else {
					info = checkEdgeIntersection(a, b);
				}
			}
			return info;
		}
	}
	
	/** Does not support the situation when a is completely inside b **/
	private static CollisionInfo checkForVertexInside(Triangle a, Triangle b) {
		Vector2f[] aVerts = a.getVertices();
		Vector2f[] bVerts = b.getVertices();
		
		CollisionInfo info = null;
		
		//check for simple collision of a vertex of a in b
		Vector2f p2 = bVerts[0];
		Vector2f q2 = bVerts[1];
		Vector2f r2 = bVerts[2];
		Vector2f p1;
		Vector2f q1;
		Vector2f r1;
		//check for simple collision of a vertex of a in b
		for (int aVertIdx = 0; aVertIdx < 3; aVertIdx++) {
			p1 = aVerts[aVertIdx];
			q1 = aVerts[(aVertIdx+1)%3];
			r1 = aVerts[(aVertIdx+2)%3];
			float a1 = p1.cross(p2, q2);
			float a2 = p1.cross(q2, r2);
			float a3 = p1.cross(r2, p2);
			// > 0 means CCW , < 0 means CW
			if (a1 >= 0 && a2 >= 0 && a3 >= 0) { // Then p1 is inside b
				for (int i = 0; i < 3; i++) { // Look for edge intersecting between p1-q1
					float[] interQ = Vector2f.findIntersection(p1, q1, bVerts[i],
							bVerts[(i + 1) % 3]);
					if (interQ[0] >= 0 && interQ[0] <= 1 && interQ[1] >= 0 && interQ[1] <= 1) {
						info = createInfoFromIntersections(interQ, i, p1, p1, b);
						if (info == null) {
							//then both p1 and r1 are inside b, r1-to-q1 must
							//intersect with an edge if the triangles
							info = createInfoFromIntersections(interQ, i, p1, r1, b);
						}
					}
					if (info != null) break;
				}
				if (info == null) {
					//then both p1 and q1 are inside b, q1-to-r1 must
					//intersect with an edge if the triangles do not share a side
					for (int i = 0; i < 3; i++) { // Look for edge intersecting between p1-r1						
						float[] interR = Vector2f.findIntersection(p1, r1, bVerts[i],
								bVerts[(i + 1) % 3]);
						if (interR[0] >= 0 && interR[0] <= 1 && interR[1] >= 0 && interR[1] <= 1) {
							info = createInfoFromIntersections(interR, i, p1, r1, b);
							break;
						}						
					}
				}
				if (info != null) break;
			}
		}
		if (info != null) {
			if (info.depth > EPSILON){
				info.positionB = new Vector2f(info.positionA);
				info.positionB.sumScale(info.normal, info.depth);
			} else info = null;
		} 
		return info;
	}
	
	/** After finding one edge intersection, this attempts to find the other, and if so
	 * generates CollisionInfo assuming that the vertex collided with the edge in otherTri it
	 * is closest to.  This vertex is p1 if the first edge is closest to p1, and p2 if it is
	 * closer.  If only one vertex is inside otherTri, p1=p2.
	 */
	private static CollisionInfo createInfoFromIntersections(float[] interFirst, int firstEdge,
			Vector2f p1, Vector2f p2, Triangle otherTri) {
		Vector2f[] otherVec = otherTri.getVertices();
		CollisionInfo info = null;
		for (int i = 0; i < 3; i++) {
			float[] inter2 = Vector2f.findIntersection(p1, p2, otherVec[i],
					otherVec[(i + 1) % 3]);
			if (inter2[0] >= 0 && inter2[0] <= 1 && inter2[1] >= 0 && inter2[1] <= 1) {
				info = new CollisionInfo();
				float depth1 = p1.distanceToLine(otherVec[firstEdge], otherVec[(firstEdge + 1) % 3]);
				float depth2 = p2.distanceToLine(otherVec[i], otherVec[(i + 1) % 3]);
				if (depth1 <= depth2) {
					info.positionA = p1;
					info.depth = depth1;
					info.normal = new Vector2f(otherTri.getNormals()[firstEdge]);
				} else {
					info.positionA = p2;
					info.depth = depth2;
					info.normal = new Vector2f(otherTri.getNormals()[i]);
				}
				break;
			}
		}
		return info;
	}
	
	/** Attempts to detect collisions where no vertex is inside the other triangle. **/
	private static CollisionInfo checkEdgeIntersection(Triangle ta, Triangle tb) {
		Vector2f[] aVerts = ta.getVertices();
		Vector2f[] bVerts = tb.getVertices();
		Vector2f av, bv, cv, dv;
		float[] abcdInter;
		List<int[]> collisionEdges = new ArrayList<int[]>();
		CollisionInfo info = null;
		for (int i = 0; i < 3; i++) {
			av = aVerts[i];
			bv = aVerts[(i+1)%3];
			for (int j = 0; j < 3; j++) {
				cv = bVerts[j];
				dv = bVerts[(j+1)%3];
				abcdInter = Vector2f.findIntersection(av, bv, cv, dv);
				if (abcdInter[0] >= 0 && abcdInter[0] <= 1
						&& abcdInter[1] >= 0 && abcdInter[1] <= 1) {
					collisionEdges.add(new int[] {i, j});					
				}				
			}
		}
		int edgecount = collisionEdges.size();
		if (edgecount == 1) {
			//A vertex is on the other triangle's edge
			return null;
			
		} else if (collisionEdges.size() == 4) {
			//either ta collided with tb or tb collided with ta.
			float taDist, tbDist;
			//which vertex collided, assuming ta hit tb
			int taIdx;
			//assuming tb hit ta
			int tbIdx;
			boolean flipped = false;
			info = new CollisionInfo();
			
			//determine taIdx
			int ta1 = collisionEdges.get(0)[0];
			int ta2 = collisionEdges.get(1)[0];
			if (ta1 == ta2) {
				ta2 = collisionEdges.get(2)[0];
			}
			switch (ta1+ta2) {
				case 1:
					taIdx = 1; break;
				case 2:
					taIdx = 0; break;
				case 3:
					taIdx = 2; break;
				default: throw new IllegalStateException();
			}
			//determine tbIdx
			int tb1 = collisionEdges.get(0)[1];
			int tb2 = collisionEdges.get(1)[1];
			if (tb1 == tb2) {
				tb2 = collisionEdges.get(2)[1];
			}
			switch (tb1+tb2) {
				case 1:
					tbIdx = 1; break;
				case 2:
					tbIdx = 0; break;
				case 3:
					tbIdx = 2; break;
				default: throw new IllegalStateException();
			}
			
			//determine orientation of collision
			Vector2f taP1 = new Vector2f(aVerts[taIdx]);
			Vector2f tbP1 = new Vector2f(bVerts[tbIdx]);
			float orient = taP1.cross(tbP1, bVerts[(tbIdx + 1)%3]);
			if ( orient > EPSILON) {
				taDist = taP1.distanceToLine(tbP1, bVerts[(tbIdx + 1)%3]);
				tbDist = tbP1.distanceToLine(taP1, aVerts[(taIdx + 2)%3]);
				if (taDist <= tbDist) {
					info.depth = taDist;
					info.normal = tb.getNormals()[(tbIdx + 1)%3];
					info.positionA = taP1;
				} else {
					info.depth = tbDist;
					info.normal = ta.getNormals()[(taIdx + 2)%3];
					info.positionA = tbP1;
					flipped = true;
				}
			} else if (orient < -EPSILON) {
				taDist = taP1.distanceToLine(tbP1, bVerts[(tbIdx + 2)%3]);
				tbDist = tbP1.distanceToLine(taP1, aVerts[(taIdx + 1)%3]);
				if (taDist <= tbDist) {
					info.depth = taDist;
					info.normal = tb.getNormals()[(tbIdx + 2)%3];
					info.positionA = taP1;
				} else {
					info.depth = tbDist;
					info.normal = ta.getNormals()[(taIdx + 1)%3];
					info.positionA = tbP1;
					flipped = true;
				}
			} else  { //they're colinear, do nothing
				return null;
			}
			info.positionB = new Vector2f(info.positionA);
			info.positionB.sumScale(info.normal, -1);
			if (flipped) info.flip();
			return info;
		} else {
			if (collisionEdges.size() > 0) System.out.println("Intersecting Edges: " + collisionEdges.size());
			return null;
		}
	}

	/**
	 * This is only used if the USE_DEVILLIERS constant is set to true.
	 * Devilliers-Guigue Algorithm found at
	 * http://citeseerx.ist.psu.edu/viewdoc
	 * /download?doi=10.1.1.6.1725&rep=rep1&type=pdf
	 */
	public static CollisionInfo getCollisionDevilliers(Triangle a, Triangle b) {

		Vector2f[] aVerts = a.getVertices();
		Vector2f[] bVerts = b.getVertices();
		boolean collision = false;
		boolean flipped = false;
		CollisionInfo info = null;
		
		Vector2f p1 = aVerts[0];
		Vector2f q1 = aVerts[1];
		Vector2f r1 = aVerts[2];
		Vector2f p2 = bVerts[0];
		Vector2f q2 = bVerts[1];
		Vector2f r2 = bVerts[2];

		// p1 inside Triangle b?
		float a1 = p1.cross(p2, q2);
		float a2 = p1.cross(q2, r2);
		float a3 = p1.cross(r2, p2);
		// > 0 means CCW , < 0 means CW
		if (a1 >= 0 && a2 >= 0 && a3 >= 0) { // Then p1 is inside b
			for (int i = 0; i < 3; i++) {
				// Look for edge intersecting between p1-q1
				float[] interQ = Vector2f.findIntersection(p1, q1, bVerts[i],
						bVerts[(i + 1) % 3]);
				if (interQ[0] >= 0 && interQ[0] <= 1 && interQ[1] >= 0
						&& interQ[1] <= 1) {
					for (int j = 0; j < 3; j++) {
						// Look for edge intersecting between p1-r1
						float[] interR = Vector2f.findIntersection(p1, r1,
								bVerts[j], bVerts[(j + 1) % 3]);
						if (interR[0] >= 0 && interR[0] <= 1 && interR[1] >= 0
								&& interR[1] <= 1) {
							info = new CollisionInfo();
							info.positionA = p1;
							if (i == j) {
								info.depth = interQ[0];
								info.normal = new Vector2f(b.getNormals()[i]);
							} else if (interQ[0] < interR[0]) {
								info.depth = interQ[0];
								info.normal = new Vector2f(b.getNormals()[i]);
							} else {
								info.depth = interR[0];
								info.normal = new Vector2f(b.getNormals()[j]);
							}
						}
					}
				}
			}
		} else { // put p1 into R1 or R2
			Vector2f tmp;
			int rots = 0;
			while (!(a1 > 0 && a3 <= 0)) {
				rots++;
				tmp = p2;
				p2 = q2;
				q2 = r2;
				r2 = tmp;
				a1 = p1.cross(p2, q2);
				a2 = p1.cross(q2, r2);
				a3 = p1.cross(r2, p2);
			}
			
			if (a2 > 0) { // p1 is in R1
				// Test I
				if (r2.cross(p2, q1) < 0) {
					// Test II.b and Test III.b and Test IV.b
					if (r2.cross(p2, r1) >= 0 && q1.cross(r1, r2) >= 0
							&& p1.cross(p2, r1) >= 0) {
						info = new CollisionInfo();
						float t1 = r1.distanceToLine(r2, p2);
						float t2 = p2.distanceToLine(p1, r1);
						if (t1 < t2) {
							info.positionA = r1;
							info.depth = t1;
							info.normal = new Vector2f(b.getNormals()[(2 + rots) % 3]);
						} else {
							info.positionA = p2;
							info.depth = t2;
							info.normal = new Vector2f(a.getNormals()[2]);
							flipped = true;
						}
						System.out.println("IV.b");
					}
					// Test II.a
				} else if (r2.cross(p1, q1) >= 0) {
					// Test III.a
					if (p1.cross(p2, q1) >= 0) {
						info = new CollisionInfo();
						float t1 = q1.distanceToLine(p2, r2);
						float t2 = r2.distanceToLine(p1, q1);
						if (t1 < t2) {
							info.positionA = q1;
							info.depth = t1;
							info.normal = new Vector2f(b.getNormals()[(rots+2)%3]);
						} else {
							info.positionA = r2;
							info.depth = t2;
							info.normal = new Vector2f(a.getNormals()[0]);
							flipped = true;
						}

						System.out.println("III.a");
					} else if (p1.cross(p2, r1) >= 0 && q1.cross(r1, p2) >= 0) {
						// Test IV.a and V
						info = new CollisionInfo();
						float t2 = p2.distanceToLine(q1, r1);
						info.positionA = p2;
						info.depth = t2; 
						info.normal = a.getNormals()[1];
						flipped = true;
						System.out.println("IV.a&V");
					}
				}
			} else { // p1 is in R2
				// Test I
				if (r2.cross(p2, q1) < 0) {					
					//Test II.b
					if (r2.cross(p2, r1) >= 0) {
						//Test III.c
						if (q1.cross(r1, r2) < 0) {
							//Tests IV.e and V.c
							if (q1.cross(r1, q2) >= 0 && q2.cross(r2, r1) >= 0) {
								info = new CollisionInfo();
								info.positionA = r1;
								info.depth = r1.distanceToLine(q2, r2);
								info.normal = b.getNormals()[(2+rots) % 3];
							}
						} else if (r1.cross(p1, p2) >= 0 ){//Test IV.d
							info = new CollisionInfo();
							float t2 = r2.distanceToLine(q1, r1);
							info.positionA = r2;
							info.depth = t2;
							info.normal = new Vector2f(a.getNormals()[1]);
							flipped = true;
						}
					}
				} else {
					//Test II.a
					if (q2.cross(r2, q1) < 0) { 						
						//Tests III.b, IV.c, and V.b
						collision = p1.cross(q2, q1) <= 0 && q2.cross(r2, r1) >= 0 &&
								q1.cross(r1, q2) >= 0;
								if (collision) System.out.println("V.b");
					} else {
						//Test III.a
						if (p1.cross(p2, q1) < 0) {							
							//Tests IV.b and V.a
							collision = p1.cross(p2, r1) >= 0 && r2.cross(p2, r1) >= 0;
							if (collision) System.out.println("V.a");
						} else if (p1.cross(q2, q1) <= 0) {
								//Test IV.a
							info = new CollisionInfo();
							float t2 = p2.distanceToLine(p1, r1);
							info.positionA = p2;
							info.depth = t2;
							info.normal = new Vector2f(a.getNormals()[2]);
							flipped = true;
							System.out.println("IV.a");
						}
					}
				}
			}
		}
		if (info != null && info.depth > EPSILON) {
			if (flipped)
				info.normal.scale(-1);
			info.positionB = new Vector2f(info.positionA);
			info.positionB.sumScale(info.normal, info.depth);

		}
		//return null;
		return info;
	}
}
